#ifndef _CONTROLLER_H__
#define _CONTROLLER_H__
#include "foc.h"
#include "utils/utils.h"
#include "lineramp.h"
/* 控制模式定义 */
#define CTRL_MODE_OPEN                 ((u8)0U)
#define CTRL_MODE_VELOCITY             ((u8)1U)
#define CTRL_MODE_TORQUE               ((u8)2U)
#define CTRL_MODE_CURRENT              ((u8)3U)

typedef struct {
    bool    b_start;
    foc_t   foc;
    u8      mode_req;
    u8      mode_running;
    float   encoder_angle;
    float   encoder_velocity;
    float   phase_curr[3];
    albt_t  curr_albt_real;
    pi_t    pi_velocity;
    pi_t    pi_velocity_limit;
	float   vbus_vol_sampled;
	float   vbus_curr_sampled;
	lineramp_t torque_target;
	lineramp_t curr_target[2];
	lineramp_t vel_target;
	lineramp_t vdq_target[2];
}controller_t;

void contrl_init(controller_t *ctrl);
void contrl_get_phase_duty(controller_t *ctrl);
void contrl_update_target(controller_t *ctrl);
bool contrl_enable(controller_t *ctrl, bool start);
bool contrl_request_mode(controller_t *ctrl, u8 mode);
u8 contrl_update_mode(controller_t *ctrl);

__STATIC_INLINE bool contrl_start(controller_t *ctrl, u8 mode) {
	if (contrl_enable(ctrl, true)){
        return contrl_request_mode(ctrl, mode);
    }
	return false;
}

__STATIC_INLINE bool contrl_stop(controller_t *ctrl) {
	return contrl_enable(ctrl, false);
}

__STATIC_INLINE bool contrl_is_start(controller_t *ctrl) {
	return ctrl->b_start;
}

__STATIC_INLINE void contrl_torque_to_idq(controller_t *ctrl, float torque, float *id, float *iq) {
	/* use the id=0 controller */
	*id = 0;
	*iq = torque;
}

__STATIC_INLINE void contrl_set_target_torque(controller_t *ctrl, float torque) {
	line_ramp_set_target(&ctrl->torque_target, torque);
}

__STATIC_INLINE void contrl_set_target_velocity(controller_t *ctrl, float vel) {
	line_ramp_set_target(&ctrl->vel_target, vel);
}

__STATIC_INLINE void contrl_set_target_idq(controller_t *ctrl, float id, float iq) {
	line_ramp_set_target(&ctrl->curr_target[0], id);
	line_ramp_set_target(&ctrl->curr_target[1], iq);
}

__STATIC_INLINE void contrl_set_target_vdq(controller_t *ctrl, float vd, float vq) {
	line_ramp_set_target(&ctrl->vdq_target[0], vd);
	line_ramp_set_target(&ctrl->vdq_target[1], vq);
}

#endif /* _CONTROLLER_H__ */

